//
// Created by 李家豪 on 2019/6/13.
//

#include "stdint.h"
#include "stdlib.h"
#include "MultiWii.h"
#include "PanicControl.h"
#include "stdbool.h"

const uint16_t panicChannel = 7;
const uint16_t panicAngle = 500; // mil-deg
const uint16_t panicGravityLowerBound = 256; // 0.5g
const uint16_t panicGravityHigherBound = 1280; // 2.5g
static bool isPanicking = false;
static bool isManualPanicking = false;

/**
 * This function should be called every frame to
 * have auto panicking and manual panicking feature.
 */
void panic_control() {
	if (isPanicking) {
		go_disarm(); // prevents rearming until recovered from panic
		if (isManualPanicking && rcData[panicChannel] < 1350) {
			relax();
		} else if (rcData[panicChannel] > 1750) {
			isManualPanicking = true;
		}
		return;
	}
	if (rcData[panicChannel] > 1750) {
		// manual panic
		isManualPanicking = true;
		panic();
	}
	// test for angle
	if (abs(att.angle[ROLL]) > panicAngle || abs(att.angle[PITCH]) > panicAngle) {
		// auto panic (stalled)
		panic();
	}
	// test for gravity
	if (imu.accSmooth[2] < panicGravityLowerBound || imu.accSmooth[2] > panicGravityHigherBound) {
		panic();
	}
	// TODO: test for gyroscope jitter
}

/**
 * Panic! Kills motor immediately.
 */
void panic() {
	GPIOE->BRR = GPIO_Pin_5; // = GPIO_SetBits(GPIOE, GPIO_Pin_5);
	go_disarm();
	isPanicking = true;
}

/**
 * Recover from panic state.
 */
void relax() {
	GPIOE->BSRR = GPIO_Pin_5; // = GPIO_ResetBits(GPIOE, GPIO_Pin_5);
	isPanicking = false;
	isManualPanicking = false;
}
